/**
 * \file
 * \date 18.09.2013
 * \author alex
 */
#include <bullet_rviz_test/robot_visualization.h>
#include <math.h>
#include <bullet/btBulletCollisionCommon.h>
#include <bullet/btBulletDynamicsCommon.h>
#include <bullet_rviz_test/robot.h>

namespace bullet_rviz_test
{

RobotVisualization::RobotVisualization(const RobotPtr & robot) :
    robot_(robot), cost_(0)
{
  robot_->registerMotionCallback(
      PhysicalObject::MotionCallback(
          boost::bind(&RobotVisualization::visualize, this)));
}

RobotVisualization::~RobotVisualization()
{
}

void RobotVisualization::visualize()
{
  btTransform world_transform;
  robot_->getRigidBody()->getMotionState()->getWorldTransform(world_transform);
  visualization_msgs::MarkerPtr marker = createMarker(world_transform);

  btVector3 box_half_extents = robot_->getHalfExtents();

  marker->type = visualization_msgs::Marker::CUBE;
  marker->scale.x = box_half_extents.x() * 2;
  marker->scale.y = box_half_extents.y() * 2;
  marker->scale.z = box_half_extents.z() * 2;

  marker->color.r = cost_;
  marker->color.g = 1 - cost_;
  marker->color.b = 0;
  marker->color.a = 1;

  publishMarker(marker);
}

void RobotVisualization::setCost(float cost)
{
  if (isnan(cost) || cost < 0 || cost > 1)
    cost_ = 1;
  else
    cost_ = cost;
}

}
